/* WiFi station Example with Multicast UDP Communication and Digital Signature Verification

   This example code is in the Public Domain (or CC0 licensed, at your option.)

   Unless required by applicable law or agreed to in writing, this
   software is distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR
   CONDITIONS OF ANY KIND, either express or implied.
*/
#include <string.h>
#include <inttypes.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "esp_system.h"
#include "esp_wifi.h"
#include "esp_event.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "cJSON.h"

#include "lwip/err.h"
#include "lwip/sys.h"
#include "lwip/sockets.h"
#include "lwip/netdb.h"

// 添加mbedtls相关头文件
#include "mbedtls/pk.h"
#include "mbedtls/entropy.h"
#include "mbedtls/ctr_drbg.h"
#include "mbedtls/sha256.h"
#include "mbedtls/rsa.h"
#include "mbedtls/error.h"
#include "mbedtls/platform.h"

// 引入MAVLink处理模块
#include "mavlink_handler.h"
#include "task_tracker.h"

#include "read_nfc_data.h"  // 包含NFC数据读取头文件

/* The examples use WiFi configuration that you can set via project configuration menu

   If you'd rather not, just change the below entries to strings with
   the config you want - ie #define EXAMPLE_WIFI_SSID "mywifissid"
*/
#define EXAMPLE_ESP_WIFI_SSID      CONFIG_ESP_WIFI_SSID
#define EXAMPLE_ESP_WIFI_PASS      CONFIG_ESP_WIFI_PASSWORD
#define EXAMPLE_ESP_MAXIMUM_RETRY  CONFIG_ESP_MAXIMUM_RETRY

#if CONFIG_ESP_WPA3_SAE_PWE_HUNT_AND_PECK
#define ESP_WIFI_SAE_MODE WPA3_SAE_PWE_HUNT_AND_PECK
#define EXAMPLE_H2E_IDENTIFIER ""
#elif CONFIG_ESP_WPA3_SAE_PWE_HASH_TO_ELEMENT
#define ESP_WIFI_SAE_MODE WPA3_SAE_PWE_HASH_TO_ELEMENT
#define EXAMPLE_H2E_IDENTIFIER CONFIG_ESP_WIFI_PW_ID
#elif CONFIG_ESP_WPA3_SAE_PWE_BOTH
#define ESP_WIFI_SAE_MODE WPA3_SAE_PWE_BOTH
#define EXAMPLE_H2E_IDENTIFIER CONFIG_ESP_WIFI_PW_ID
#endif
#if CONFIG_ESP_WIFI_AUTH_OPEN
#define ESP_WIFI_SCAN_AUTH_MODE_THRESHOLD WIFI_AUTH_OPEN
#elif CONFIG_ESP_WIFI_AUTH_WEP
#define ESP_WIFI_SCAN_AUTH_MODE_THRESHOLD WIFI_AUTH_WEP
#elif CONFIG_ESP_WIFI_AUTH_WPA_PSK
#define ESP_WIFI_SCAN_AUTH_MODE_THRESHOLD WIFI_AUTH_WPA_PSK
#elif CONFIG_ESP_WIFI_AUTH_WPA2_PSK
#define ESP_WIFI_SCAN_AUTH_MODE_THRESHOLD WIFI_AUTH_WPA2_PSK
#elif CONFIG_ESP_WIFI_AUTH_WPA_WPA2_PSK
#define ESP_WIFI_SCAN_AUTH_MODE_THRESHOLD WIFI_AUTH_WPA_WPA2_PSK
#elif CONFIG_ESP_WIFI_AUTH_WPA3_PSK
#define ESP_WIFI_SCAN_AUTH_MODE_THRESHOLD WIFI_AUTH_WPA3_PSK
#elif CONFIG_ESP_WIFI_AUTH_WPA2_WPA3_PSK
#define ESP_WIFI_SCAN_AUTH_MODE_THRESHOLD WIFI_AUTH_WPA2_WPA3_PSK
#elif CONFIG_ESP_WIFI_AUTH_WAPI_PSK
#define ESP_WIFI_SCAN_AUTH_MODE_THRESHOLD WIFI_AUTH_WAPI_PSK
#endif

/* FreeRTOS event group to signal when we are connected*/
static EventGroupHandle_t s_wifi_event_group;

/* The event group allows multiple bits for each event, but we only care about two events:
 * - we are connected to the AP with an IP
 * - we failed to connect after the maximum amount of retries */
#define WIFI_CONNECTED_BIT BIT0
#define WIFI_FAIL_BIT      BIT1

static const char *TAG = "wifi station";
static const char *TAGCHECK = "CHECK RESPONSE";
static int s_retry_num = 0;

// Multicast UDP 通信相关定义
#define MCAST_GRP "239.10.10.1"
#define MCAST_PORT 5001


// 设备ID (攻击无人机范围: 0x00000000 ~ 0xFF00FFFF)
uint32_t g_device_id = 0x00000000;
#define DEVICE_ID 0x00000004
static uint8_t current_ammo_type = 1;    // 弹药类型（0:空载 1:穿甲弹...）
static uint16_t mount_point = 0;         // 挂点编号（默认为0）

// 添加NFC状态跟踪变量
static bool nfc_enabled = false;
static uint32_t last_nfc_update = 0;

// 消息类型定义
#define MSG_FLEET_BROADCAST   0x1001
#define MSG_DRONE_JOIN_REQ    0x1002
#define MSG_GROUND_CONNECT    0x1003
#define MSG_DRONE_JOIN_ACK    0x1012
#define MSG_GROUND_CONNECT_ACK 0x1013
#define MSG_DRONE_STATUS      0x2001
// 添加任务相关消息类型
#define MSG_TASK_COMMAND      0x4001    // 任务指令
#define MSG_TASK_RESPONSE     0x4011    // 任务响应

// 消息头定义
#define MSG_HEADER1 0xEB
#define MSG_HEADER2 0x90

#define RSA_SIGNATURE_LEN     256     // RSA-2048签名长度

// 心跳检测和自动重连配置
#define REJOIN_RETRY_INTERVAL_MS  3000    // 重新入群申请间隔3秒
#define MAX_REJOIN_ATTEMPTS       5       // 最大重试次数

// 原始消息结构体（用于解析网络数据）
typedef struct {
    uint8_t header1;        // 0xEB
    uint8_t header2;        // 0x90
    uint32_t sender_id;     // 发送方设备ID (网络字节序)
    uint8_t seq_num;        // 循环序号
    uint16_t msg_flag;      // 消息标志 
    uint16_t content_len;   // 内容长度 
} __attribute__((packed)) msg_header_t;

typedef enum {
    FLEET_STATE_DISCONNECTED,    // 断连状态
    FLEET_STATE_JOINING,         // 正在入群
    FLEET_STATE_CONNECTED        // 已连接
} fleet_connection_state_t;

static fleet_connection_state_t fleet_state = FLEET_STATE_DISCONNECTED;

// 线程安全的全局变量保护
static portMUX_TYPE broadcast_tick_mutex = portMUX_INITIALIZER_UNLOCKED;
static volatile uint32_t last_broadcast_tick = 0;

SemaphoreHandle_t task_tracker_mutex = NULL;

#define BROADCAST_TIMEOUT_TICKS pdMS_TO_TICKS(5000)

// 重连状态追踪
static struct {
    uint32_t last_rejoin_attempt;     // 上次重连尝试时间
    uint8_t rejoin_attempt_count;     // 当前重试计数
    bool is_rejoining;                // 是否正在重连中
} rejoin_tracker = {0};

// 在文件开头添加攻击任务状态定义
typedef enum {
    ATTACK_STATE_IDLE,          // 空闲
    ATTACK_STATE_START,
    ATTACK_STATE_THROW,
    ATTACK_STATE_GUIDE,         // GUIDE
    ATTACK_STATE_ARMING,        // 解锁中
    ATTACK_STATE_TAKEOFF,       // 起飞中  
    ATTACK_STATE_MISSION_MODE,  // 切换航线模式
    ATTACK_STATE_EXECUTING,     // 执行中
    ATTACK_STATE_COMPLETED      // 完成
} attack_state_t;

typedef struct {
    attack_state_t state;
    uint32_t start_time;        // 开始时间(tick)
    uint32_t last_action_time;  // 上次动作时间
    bool is_active;             // 是否激活
} attack_task_manager_t;

typedef struct {
    bool is_executing;           // 是否正在执行攻击任务
    uint16_t task_code;         // 任务代码
    uint16_t target_id;         // 目标ID
    attack_state_t state;       // 当前状态
    uint32_t start_time;        // 开始时间
    uint32_t last_action_time;  // 上次动作时间
    uint32_t timeout_limit;     // 超时限制(ms)
    bool armed_confirmed;       // 解锁确认标志
    bool takeoff_confirmed;     // 起飞确认标志
    bool mission_started;       // 航线开始标志
} attack_execution_tracker_t;

static attack_execution_tracker_t attack_exec_tracker = {0};


// 全局变量
static attack_task_manager_t attack_manager = {0};


static drone_task_t current_task;
static bool has_mission_data = false;
// 提供函数供handler调用current_task和has_mission_data状态


static uint8_t seq_counter = 0;
static int udp_sock = -1;

// 数字签名验证相关变量
static mbedtls_pk_context pk_verify;
static mbedtls_entropy_context entropy;
static mbedtls_ctr_drbg_context ctr_drbg;
static bool signature_initialized = false;

// 公钥PEM格式 - 与brocast.py中的私钥对应
static const char* public_key_pem = 
"-----BEGIN PUBLIC KEY-----\n"
"MIIBIjANBgkqhkiG9w0BAQEFAAOCAQ8AMIIBCgKCAQEA05N3pZg+Azf/lXCScGAO\n"
"okah68ExmoeQ4FRIfrtzRz++J1sV7BiEDS08yavlQvp3dEU7K387uBJq3ELfBm6W\n"
"Q0LdaHIknLzMAUwzOx57qQm933iXsCVPRHEmQfIo2qufViXYOIftFRiQe4CDdR9v\n"
"wEVg9XEIlYV0SzVqbHGA6dvPBWzlhGQiUpJYQdil/Xfk1Sux0gcb77U3NaFqnQkJ\n"
"zD1qh2X4n3Uvbg6APuyWWna0h6UdpXSAl/4Oe9OMK9ONiEmcIrbK2xcE8mDewjxe\n"
"kmI7NAy9JUj8Pm782OBMu5i3sKPLXOiv3cUmhob1hTvgbr2XzqGhtPjN7tEKyIqM\n"
"ywIDAQAB\n"
"-----END PUBLIC KEY-----";

// 在文件开头添加签名相关的静态变量和函数声明
static mbedtls_pk_context pk_sign;
static bool signature_sign_initialized = false;


static const char* private_key_pem = 
"-----BEGIN RSA PRIVATE KEY-----\n"
"MIIEpAIBAAKCAQEAtdHNfzogSljjlH0GR9OC4hzGYsMApoRyKShkdr1yh4RwAPyK\n"
"YypBOuuNObdF1nph/cF5vErAFIww3CWMIz/yGntnKTTo5NFN9rGf8bC+6cL3wPcO\n"
"rQOYbEIsZIGMnMvJ1PQKsJm8OMPBT4wCo/JkDywVDQbcMOUql+Tn/MRLUCVcuuSY\n"
"wWn2yG4PxUCGIerK9Rh/7nrGY5ribIiUebtiLQcc6jnoGAa1/eUYwuNwdwnKtyr1\n"
"JcBZWGBlEQmmVLsumgj/BSa5I3f+3BiEqR1B2VAEUcp9qYv6mc8dSI+aRoaf88zA\n"
"fa2lYf4gGqfCBOMTt9TmdHpr4EY883r4mkB6bwIDAQABAoIBAEI23Mq3pWjOBFtg\n"
"SLhBrAmihxESiRsbYuPgRO4FQj9eGNgTIGbKzaSigfUB7JoWSbd7DVVoN5mEEIsr\n"
"1WRE9mt0g3Y7QZGLNARiXahMQMqq6UVJ8/tIMutCEh6sG0RC5wb5tSbpZeWaTL0m\n"
"dRO1w4DPAf+Tfv8DhkZ4449nATJ3O4mKDjQMnAoICx+qSkFzCsiDDTrw7GnrkCXD\n"
"LJo34p0RGERqipiP5nHe40X89aLhwHA7HY5RUhGU/sNwmQ772o/pzywqGpZKTRoX\n"
"PpyQ2ivtqTu7ZiQuSxDcc4PxKiGyAnSR5n69H8PGB7/BdQ59LLh4lL5PI+gH4Zxl\n"
"iObDSdECgYEAzmQN79ctH3bBN18n0pxDzx56tLRcf9byRdOuTodSvMsfJfqwdyQR\n"
"Miys8+4VDRYzuro5IfmN3ZLJKJrGmeZ0MUprys4ITCItjTw39e/IG7lIWaklbe1a\n"
"YhR+SWqCzVZnvOzQEDI1FCNozmDnPm1qw4+oEJESoV9uMPJPMzrGEKkCgYEA4YXK\n"
"gokt2xO4ZUoTUU5QJ0XsR5BrO0npCu7coQoedJGFvfa/M0mXSTZXa26NPenmpD6X\n"
"20mkRcnTr0/8vorSBrLm07k5O6ZmImsykpDhJh3i8w5HUQ/HEnmWKDlX5KpmQbjM\n"
"965Q7dKVlPALnAl8a0E/4f0QQ4CvcRLeFB7v6VcCgYEAlsTqlXFoChsTbKK7kZQL\n"
"yQrn8ofrJoxb8XyAUedzHm13tKPcMzyKv095jLw4bpXwdhDB2brW0qz+/AWM3TJH\n"
"YRaPhdhaoVs6UXlTG/rtAKuqWa1BpXsFUq7Lzk/be6sKHy4TlDFJKwvu7QxHAQin\n"
"PHL+ABQqOu5erazlBHSD6fkCgYApDm1WpFinyIhKUFkB9UrcS3t/b8hFy+fUKLYb\n"
"zzbKK+/4LGKUmLIlDvKNSVwHSHzHq+mDb32rgtm50csGeUlRFOK26sKDnIB9hZx4\n"
"WE3U3Z4Cd7f+BuxL7byrZ/0bUPY6wsT11lM/aJX4vlauun9fG0rg/mwmjdfisveh\n"
"HBy7EwKBgQCWAsJKm4gSFsWJKm3rz/J8AF34gBiODsYvMnUrDcsPyA8hPOZcpUTF\n"
"EPm7VPIm/k34TNU7ISEuvIirWOzmbl72k8j5edtolIYWjtz7nc0Hl0WcZsu0IOML\n"
"Y2opMutRDHZDwDpHU/PRmDLkcRTRpljAYiR+ZCnVym8vit4lWPQlLA==\n"
"-----END RSA PRIVATE KEY-----";
const uint16_t DEFAULT_TARGET_ID = 0x0000;

// 全局变量保存对方地址
static struct sockaddr_in remote_addr;

// static struct sockaddr_in local_addr;
static uint32_t local_ip;
// ==================== 飞控相关状态（通过MAVLink解析获取） ====================
static bool is_self_check_passed = false; // 自检状态（从飞控获取）
static bool is_unlocked = false;         // 解锁状态（从飞控获取）
static bool has_target = false;          // 是否装订目标（从飞控获取）


// ==================== 机群管理相关状态（通过UDP消息获取） ====================
static uint16_t current_target_id = 2;    // 当前目标编号
static uint8_t current_target_type = 1;  // 目标类型（0:未知 1:碉堡...）
static double current_target_lon = 0.0;   // 目标经度（度）
static double current_target_lat = 0.0;   // 目标纬度（度）
static float current_target_alt = 0.0;    // 目标海拔高度（米）

static bool need_guide_ack = false;  // 总是需要航点GPS确认
static bool need_mission_ack = false;  // 有航点时需要航线确认

// ===== 任务状态管理代码 =====

// 全局任务响应跟踪器
task_response_tracker_t task_tracker = {0};

// 超时时间定义（毫秒）
#define TASK_RESPONSE_TIMEOUT_MS 5000  // 5秒超时

// 组播相关
struct sockaddr_in dest_addr;
struct ip_mreq mreq;

// wifi 重连
static TaskHandle_t wifi_reconnect_task_handle = NULL;
static bool wifi_connected = false;
static SemaphoreHandle_t wifi_mutex = NULL;

// 函数声明
static bool init_signature_verification(void);
static bool verify_message_signature(uint8_t *message_data, size_t header_len, 
                                     uint8_t *signature, size_t sig_len);
static void print_hex_string(const char* label, const unsigned char* data, size_t len);
static void print_mbedtls_error(const char* function, int error_code);

static void handle_broadcast_message(uint8_t *rx_buffer, int len, msg_header_t *header);
static void handle_join_ack_message(uint8_t *rx_buffer, int len, msg_header_t *header);
static void handle_task_command_message(uint8_t *rx_buffer, int len, msg_header_t *header);

static void send_task_response(uint16_t task_code, uint8_t result, uint16_t error_code, uint16_t target_id);
static void execute_attack_task(void);
static void handle_task_command(uint8_t *content, uint16_t content_len);
bool get_current_mission_data(drone_task_t *task_data);
bool has_valid_mission_data(void);

// 状态函数声明
static void reset_task_tracker(void);
static void start_task_response_tracking(uint16_t task_code, uint16_t target_id, 
                                       bool need_guide_ack, bool need_mission_ack);
static void handle_guide_ack_response(bool success);
static void handle_mission_ack_response(bool success);
static void check_task_completion(void);
static void check_task_timeout(void);
static void on_guide_ack_received(bool success);
static void on_mission_ack_received(bool success);
static void set_fleet_state(fleet_connection_state_t new_state);

// 重新上线，清空任务
static void reset_current_task(void);
static void update_last_broadcast_tick_safe(uint32_t tick);
static uint32_t get_last_broadcast_tick_safe(void);

void print_sockaddr(const struct sockaddr *addr);

static void finish_attack_task(bool success, uint16_t error_code);
static void disarm_success(bool success, uint16_t error_code);

static void wifi_reconnect_task(void *pvParameters);
static void network_services_monitor_task(void *pvParameters);

// 添加NFC初始化函数声明
static esp_err_t nfc_system_init(void);
static void update_mount_point_from_nfc(void);


// 初始化数字签名验证
static bool init_signature_verification(void) {
    int ret = 0;
    const char* pers = "esp32_signature_verify";
    
    if (signature_initialized) {
        return true;
    }
    
    // 初始化上下文
    mbedtls_pk_init(&pk_verify);
    mbedtls_entropy_init(&entropy);
    mbedtls_ctr_drbg_init(&ctr_drbg);
    
    // ESP_LOGI(TAG, "初始化数字签名验证...");
    
    // 初始化随机数生成器
    ret = mbedtls_ctr_drbg_seed(&ctr_drbg, mbedtls_entropy_func, &entropy,
                                (const unsigned char*)pers, strlen(pers));
    if (ret != 0) {
        print_mbedtls_error("mbedtls_ctr_drbg_seed", ret);
        goto cleanup;
    }
    
    // 解析公钥
    ret = mbedtls_pk_parse_public_key(&pk_verify, (const unsigned char*)public_key_pem,
                                      strlen(public_key_pem) + 1);
    if (ret != 0) {
        print_mbedtls_error("mbedtls_pk_parse_public_key", ret);
        ESP_LOGE(TAG, "公钥解析失败，请检查公钥格式");
        goto cleanup;
    }
    
    // ESP_LOGI(TAG, "数字签名验证初始化成功");
    // ESP_LOGI(TAG, "公钥类型: %s", mbedtls_pk_get_name(&pk_verify));
    // ESP_LOGI(TAG, "密钥长度: %d bits", (int)mbedtls_pk_get_bitlen(&pk_verify));
    
    signature_initialized = true;
    return true;
    
cleanup:
    mbedtls_pk_free(&pk_verify);
    mbedtls_entropy_free(&entropy);
    mbedtls_ctr_drbg_free(&ctr_drbg);
    return false;
}

// 验证消息签名
static bool verify_message_signature(uint8_t *message_data, size_t header_len, 
                                     uint8_t *signature, size_t sig_len) {
    if (!signature_initialized) {
        ESP_LOGE(TAG, "签名验证未初始化");
        return false;
    }

    // 1. 提取 "集群管理设备ID + 约定口令" 明文
    const char *passphrase = "zhimakaimen";  // 替换成你的实际约定口令
    char id_and_passphrase[64];  // 存储 "ID + 约定口令"

    // 假设发送方ID是 0xfffe0001（示例）
    snprintf(id_and_passphrase, sizeof(id_and_passphrase), "FFFE0001%s", passphrase);

    // ESP_LOGI(TAG, "待验证接收明文: %s", id_and_passphrase);

    // 2. 计算 SHA-256 哈希
    unsigned char hash[32];
    int ret = mbedtls_sha256((const unsigned char *)id_and_passphrase, 
                            strlen(id_and_passphrase), hash, 0);
    if (ret != 0) {
        print_mbedtls_error("mbedtls_sha256", ret);
        return false;
    }

    // print_hex_string("接收的明文哈希", hash, 32);

    // 3. 验证 RSA 签名
    ret = mbedtls_pk_verify(&pk_verify, MBEDTLS_MD_SHA256, hash, 32,
                            signature, sig_len);
    if (ret != 0) {
        if (ret == MBEDTLS_ERR_RSA_VERIFY_FAILED) {
            ESP_LOGW(TAG, "签名验证失败：签名不匹配");
        } else {
            print_mbedtls_error("mbedtls_pk_verify", ret);
        }
        return false;
    }

    // ESP_LOGI(TAG, "✓ 消息签名验证成功");
    return true;
}

// 添加签名初始化函数
static bool init_signature_signing(void) {
    int ret = 0;
    
    if (signature_sign_initialized) {
        return true;
    }
    
    // 初始化签名上下文
    mbedtls_pk_init(&pk_sign);
    
    mbedtls_entropy_init(&entropy);
    mbedtls_ctr_drbg_init(&ctr_drbg);
    
    const char* pers = "esp32_signature_test";
    // ESP_LOGI(TAG, "1. 初始化随机数生成器...");
    
    // 初始化随机数生成器
    ret = mbedtls_ctr_drbg_seed(&ctr_drbg, mbedtls_entropy_func, &entropy,
                                (const unsigned char*)pers, strlen(pers));
    if (ret != 0) {
        print_mbedtls_error("mbedtls_ctr_drbg_seed", ret);
        goto cleanup;
    }


    // ESP_LOGI(TAG, "初始化数字签名功能...");
    
    // 解析私钥
    ret = mbedtls_pk_parse_key(&pk_sign, (const unsigned char*)private_key_pem,
                               strlen(private_key_pem) + 1, NULL, 0,
                               mbedtls_ctr_drbg_random, &ctr_drbg);
    

    if (ret != 0) {
        print_mbedtls_error("mbedtls_pk_parse_key", ret);
        ESP_LOGE(TAG, "私钥解析失败，请检查私钥格式");
        goto cleanup;
    }
    
    // ESP_LOGI(TAG, "数字签名功能初始化成功");
    // ESP_LOGI(TAG, "私钥类型: %s", mbedtls_pk_get_name(&pk_sign));
    // ESP_LOGI(TAG, "密钥长度: %d bits", (int)mbedtls_pk_get_bitlen(&pk_sign));
    
    signature_sign_initialized = true;
    return true;
    
cleanup:
    mbedtls_pk_free(&pk_sign);
    return false;
}

// 添加消息签名函数
static bool sign_message(uint8_t *signature, size_t *sig_len) {
    if (!signature_sign_initialized) {
        ESP_LOGE(TAG, "签名功能未初始化");
        return false;
    }

    // 根据协议，签名的是 "攻击无人机设备ID + 约定口令"
    const char *passphrase = "zhimakaimen";  // 约定口令
    char id_and_passphrase[64];
    
    // 构造待签名的明文：设备ID + 约定口令
    // snprintf(id_and_passphrase, sizeof(id_and_passphrase), "%08x%s", g_device_id, passphrase);
    snprintf(id_and_passphrase, sizeof(id_and_passphrase), "%08" PRIx32 "%s", g_device_id, passphrase);
    
    // static const char* id_and_passphrase = "0x00000001zhimakaimen";

    // ESP_LOGI(TAG, "待签名的发送明文: %s", id_and_passphrase);
    

    // 计算SHA-256哈希
    unsigned char hash[32];
    int ret = mbedtls_sha256((const unsigned char *)id_and_passphrase, 
                            strlen(id_and_passphrase), hash, 0);
    if (ret != 0) {
        print_mbedtls_error("mbedtls_sha256", ret);
        return false;
    }
    
    // print_hex_string("发送的明文哈希", hash, 32);
    
    // 生成RSA签名
    ret = mbedtls_pk_sign(&pk_sign, MBEDTLS_MD_SHA256, hash, 32,
                          signature, MBEDTLS_MPI_MAX_SIZE, sig_len,
                          mbedtls_ctr_drbg_random, &ctr_drbg);
    if (ret != 0) {
        print_mbedtls_error("mbedtls_pk_sign", ret);
        return false;
    }
    
    // ESP_LOGI(TAG, "✓ 消息签名成功，签名长度: %d", *sig_len);
    // print_hex_string("签名数据", signature, *sig_len > 32 ? 32 : *sig_len);
    
    return true;
}


// 计算简单校验和
static uint16_t calculate_checksum(uint8_t *data, int len) {
    uint32_t sum = 0;
    for (int i = 0; i < len; i++) {
        sum += data[i];
    }
    return (uint16_t)(sum & 0xFFFF);
}

// 更新后的JSON解析函数
static bool parse_task_json(const char *json_str) {
    cJSON *root = cJSON_Parse(json_str);
    if (root == NULL) {
        ESP_LOGE(TAG, "JSON解析错误: %s", cJSON_GetErrorPtr());
        return false;
    }

    // 添加JSON内容打印
    char *json_print = cJSON_Print(root);
    if (json_print != NULL) {
        ESP_LOGI(TAG, "解析到的JSON内容: %s", json_print);
        free(json_print); // 记得释放内存
    }

    // 解析基础字段
    cJSON *id = cJSON_GetObjectItem(root, "target_id");
    cJSON *type = cJSON_GetObjectItem(root, "target_type");
    cJSON *photo_url = cJSON_GetObjectItem(root, "photo_url");
    // cJSON *strike_type = cJSON_GetObjectItem(root, "strike_type");

    // 添加调试输出
    ESP_LOGI(TAG, "调试信息:");
    ESP_LOGI(TAG, "id指针: %p", (void*)id);
    ESP_LOGI(TAG, "type指针: %p", (void*)type);
    ESP_LOGI(TAG, "photo_url指针: %p", (void*)photo_url);

    if (id) {
        if (cJSON_IsNumber(id)) {
            ESP_LOGI(TAG, "target_id值: %d", id->valueint);
        } else {
            ESP_LOGI(TAG, "target_id不是数字类型，类型: %d", id->type);
        }
    } else {
        ESP_LOGI(TAG, "target_id解析失败");
    }

    if (type) {
        if (cJSON_IsNumber(type)) {
            ESP_LOGI(TAG, "target_type值: %d", type->valueint);
        } else {
            ESP_LOGI(TAG, "target_type不是数字类型，类型: %d", type->type);
        }
    } else {
        ESP_LOGI(TAG, "target_type解析失败");
    }

    if (photo_url) {
        if (cJSON_IsString(photo_url)) {
            ESP_LOGI(TAG, "photo_url值: %s", photo_url->valuestring);
        } else {
            ESP_LOGI(TAG, "photo_url不是字符串类型，类型: %d", photo_url->type);
        }
    } else {
        ESP_LOGI(TAG, "photo_url解析失败");
    }

    // 原来的检查逻辑
    if (!id) {
        ESP_LOGE(TAG, "缺少必要字段 - id:%p", 
                (void*)id);
        cJSON_Delete(root);
        return false;
    }

    // 填充任务结构体
    memset(&current_task, 0, sizeof(current_task));
    current_task.target_id = id->valueint;
    // current_task.strike_type = strike_type->valueint;
    current_task.strike_type = 1;
    if (type) {
        current_task.target_type = type->valueint;
    }
    if (photo_url) {
        strncpy(current_task.photo_url, photo_url->valuestring, sizeof(current_task.photo_url)-1);
    }

    // 解析GPS信息
    cJSON *gps = cJSON_GetObjectItem(root, "target_gps");
    if (gps) {
        cJSON *alt = cJSON_GetObjectItem(gps, "alt");
        cJSON *lon = cJSON_GetObjectItem(gps, "lon");
        cJSON *lat = cJSON_GetObjectItem(gps, "lat");
        
        if (alt && lon && lat) {
            current_task.target_alt = alt->valuedouble;
            current_task.target_lon = lon->valuedouble;
            current_task.target_lat = lat->valuedouble; // 转换为数值
        }
    }

    // 解析航线点
    cJSON *air_line = cJSON_GetObjectItem(root, "air_line");
    if (air_line) {
        int array_size = cJSON_GetArraySize(air_line);
        array_size = array_size > 10 ? 10 : array_size; // 限制最大航点数
        
        for (int i = 0; i < array_size; i++) {
            cJSON *point = cJSON_GetArrayItem(air_line, i);
            cJSON *alt = cJSON_GetObjectItem(point, "alt");
            cJSON *lon = cJSON_GetObjectItem(point, "lon");
            cJSON *lat = cJSON_GetObjectItem(point, "lat");
            cJSON *speed = cJSON_GetObjectItem(point, "speed");
            
            if (alt && lon && lat && speed) {
                current_task.waypoints[i].alt = alt->valuedouble;
                current_task.waypoints[i].lon = lon->valuedouble;
                current_task.waypoints[i].lat = lat->valuedouble;
                current_task.waypoints[i].speed = speed->valuedouble;
                current_task.waypoint_count++;
            }
        }
    }

    cJSON_Delete(root);
    return true;
}

// 添加状态转换日志函数
static void set_fleet_state(fleet_connection_state_t new_state) {
    if (fleet_state != new_state) {
        const char* state_names[] = {"DISCONNECTED", "JOINING", "CONNECTED"};
        ESP_LOGI(TAG, "机群状态变化: %s -> %s", 
                state_names[fleet_state], state_names[new_state]);
        fleet_state = new_state;
    }
}

// 添加内联函数来判断连接状态
static inline bool is_connected_to_fleet(void) {
    return (fleet_state == FLEET_STATE_CONNECTED);
}

// 添加内联函数来判断是否可以发送状态
static inline bool can_send_status(void) {
    return (fleet_state == FLEET_STATE_CONNECTED);
}

// ===== 新增的状态管理函数 =====

// 重置任务跟踪器
static void reset_task_tracker(void) {
    // 重置整个结构体为0
    memset(&task_tracker, 0, sizeof(task_tracker));
    
    // 设置初始状态
    task_tracker.state = TASK_RESPONSE_IDLE;
}

// 开始跟踪任务响应
static void start_task_response_tracking(uint16_t task_code, uint16_t target_id, 
                                       bool need_guide_ack, bool need_mission_ack) {

    if (xSemaphoreTake(task_tracker_mutex, portMAX_DELAY) == pdTRUE) {
        // 检查是否有任务正在执行
        uint32_t start_current_tick = xTaskGetTickCount();
        ESP_LOGI(TAG, "新任务开始时间:(%lu ticks), ", start_current_tick);
        if (task_tracker.is_waiting) {
            ESP_LOGW(TAG, "警告：有任务正在执行中(task_code=0x%04X)，拒绝新任务(0x%04X)", 
                     task_tracker.task_code, task_code);
            
            xSemaphoreGive(task_tracker_mutex);
            
            // 立即响应任务繁忙错误
            send_task_response(task_code, 1, 0x0030, target_id); // 0x0030=设备忙
            return;
        }
        
        reset_task_tracker();
        task_tracker.is_waiting = true;
        task_tracker.task_code = task_code;
        task_tracker.target_id = target_id;
        task_tracker.guide_ack_received = !need_guide_ack;
        task_tracker.mission_ack_received = !need_mission_ack;
        task_tracker.timeout_timer = xTaskGetTickCount();
        task_tracker.state = TASK_RESPONSE_WAITING;
        
        xSemaphoreGive(task_tracker_mutex);
        
        ESP_LOGI(TAG, "开始跟踪任务响应: 任务码=0x%04X", task_code);
    }
}

// 处理GUIDE_ACK响应
static void handle_guide_ack_response(bool success) {
    if (!task_tracker.is_waiting) {
        ESP_LOGW(TAG, "收到GUIDE_ACK但未在等待状态");
        return;
    }
    
    task_tracker.guide_ack_received = true;
    task_tracker.guide_success = success;
    
    ESP_LOGI(TAG, "收到GUIDE_ACK: %s", success ? "成功" : "失败");
    
    check_task_completion();
}

// 处理MISSION_ACK响应
static void handle_mission_ack_response(bool success) {
    if (!task_tracker.is_waiting) {
        ESP_LOGW(TAG, "收到MISSION_ACK但未在等待状态");
        return;
    }
    
    task_tracker.mission_ack_received = true;
    task_tracker.mission_success = success;
    
    ESP_LOGI(TAG, "收到MISSION_ACK: %s", success ? "成功" : "失败");
    
    check_task_completion();
}

// 检查任务是否完成
static void check_task_completion(void) {
    if (xSemaphoreTake(task_tracker_mutex, portMAX_DELAY) == pdTRUE){
        static int checkNum = 0;
        ESP_LOGI(TAGCHECK, "check: %d", checkNum);
        checkNum++;
        if (!task_tracker.is_waiting) {
            ESP_LOGI(TAGCHECK, "check: %d, taask_tracker不在等待状态: %d", checkNum, task_tracker.is_waiting);
            xSemaphoreGive(task_tracker_mutex);
            return;
        }

        ESP_LOGI(TAGCHECK, "目标位置是否收到?: %d; 航线位置是否收到: %d ", task_tracker.guide_ack_received, task_tracker.mission_ack_received);
        
        // 检查是否都收到了需要的确认
        bool all_acks_received = task_tracker.guide_ack_received && 
                                task_tracker.mission_ack_received;
        
        if (!all_acks_received) {
            ESP_LOGI(TAG, "等待更多确认: GUIDE=%d, MISSION=%d", 
                    task_tracker.guide_ack_received, task_tracker.mission_ack_received);
            xSemaphoreGive(task_tracker_mutex);
            return;
        }
        ESP_LOGI(TAG, "都已收到航点和航线确认");
        // 检查是否都成功
        ESP_LOGI(TAGCHECK, "目标位置是否成功?: %d; 航线位置是否成功: %d ", task_tracker.guide_success, task_tracker.mission_success);
        
        bool all_success = task_tracker.guide_success && task_tracker.mission_success;
        
        // 发送最终响应
        uint8_t result = all_success ? 0 : 1;  // 0=成功, 1=失败
        uint16_t error_code = 0;
        
        if (!all_success) {
            if (!task_tracker.guide_success) {
                error_code = 0x0010;  // 航点GPS装订失败
            } else if (!task_tracker.mission_success) {
                error_code = 0x0011;  // 航线上传失败
            }
        }
        
        ESP_LOGI(TAG, "任务完成: %s, 错误码=0x%04X", 
                all_success ? "成功" : "失败", error_code);
        ESP_LOGI(TAG, "在发送回报之前,task_tracker.is_waiting: %d",task_tracker.is_waiting);         
        // 重置跟踪器
        send_task_response(task_tracker.task_code, result, error_code, task_tracker.target_id);
        
        if (task_tracker.task_code == 0x0001) {
            if (error_code == 0) {
                mavlink_set_has_target(true);
                mavlink_set_mission_state(true);   
            } else {
                mavlink_set_has_target(false); 
                mavlink_set_mission_state(false);
            }
        }

        if (task_tracker.task_code == 0x0002) {
            if (error_code == 0) {
                mavlink_set_has_target(false);
                mavlink_set_mission_state(false); 
            }
        }
        
        reset_task_tracker();
        xSemaphoreGive(task_tracker_mutex);

        uint32_t task_current_tick = xTaskGetTickCount();
        ESP_LOGI(TAG, "任务执行成功时间:(%lu ticks), ", task_current_tick);
        ESP_LOGI(TAG, "在发送回报之后-------,task_tracker.is_waiting: %d",task_tracker.is_waiting);
    }
}

// 检查任务响应超时
static void check_task_timeout(void) {
    if (!task_tracker.is_waiting) {
        return;
    }
    
    uint32_t current_time = xTaskGetTickCount();
    uint32_t elapsed_ms = (current_time - task_tracker.timeout_timer) * portTICK_PERIOD_MS;
    
    if (elapsed_ms > TASK_RESPONSE_TIMEOUT_MS) {
        ESP_LOGW(TAG, "任务响应超时: 已等待%" PRIu32 "ms", elapsed_ms);
        
        // 发送超时错误响应
        send_task_response(task_tracker.task_code, 1, 0x0020, task_tracker.target_id);
        
        // 重置跟踪器
        reset_task_tracker();
    }
}

// 线程安全的tick更新函数
static void update_last_broadcast_tick_safe(uint32_t tick) {
    portENTER_CRITICAL(&broadcast_tick_mutex);
    last_broadcast_tick = tick;
    portEXIT_CRITICAL(&broadcast_tick_mutex);
}

// 线程安全的tick读取函数
static uint32_t get_last_broadcast_tick_safe(void) {
    uint32_t tick;
    portENTER_CRITICAL(&broadcast_tick_mutex);
    tick = last_broadcast_tick;
    portEXIT_CRITICAL(&broadcast_tick_mutex);
    return tick;
}

/**
 * 重置当前任务状态
 * 清空所有任务相关的数据和状态标志
 */
static void reset_current_task(void) {
    ESP_LOGI(TAG, "开始重置当前任务状态...");
    
    // 1. 清零整个任务结构体
    memset(&current_task, 0, sizeof(drone_task_t));
    
    // 2. 重置任务数据标志
    has_mission_data = false;
    
    // 3. 重置目标相关的全局状态变量
    has_target = false;
    current_target_id = DEFAULT_TARGET_ID;  // 使用默认值0x0000
    current_target_type = 0;                // 未知类型
    current_target_lon = 0.0;
    current_target_lat = 0.0;
    current_target_alt = 0.0;
    
    // 4. 通知MAVLink模块更新状态
    mavlink_set_has_target(has_target);
    
    // 5. 清除MAVLink任务数据
    // mavlink_mission_clear_all();
    // mavlink_set_mission_upload_state(0, 0, MISSION_STATE_IDLE);
    
    // 6. 重置任务响应跟踪器
    reset_task_tracker();
    
    // 7. 记录重置完成日志
    // ESP_LOGI(TAG, "任务状态重置完成:");
    // ESP_LOGI(TAG, "  - 任务数据已清空");
    // ESP_LOGI(TAG, "  - 目标状态: has_target=%d", has_target);
    // ESP_LOGI(TAG, "  - 目标ID: 0x%04X", current_target_id);
    // ESP_LOGI(TAG, "  - MAVLink状态已同步");
    // ESP_LOGI(TAG, "  - 任务跟踪器已重置");
}


// 提供给mavlink_handler访问的安全函数
bool get_current_mission_data(drone_task_t *task_data) {
    if (!task_data || !has_mission_data) {
        return false;
    }
    
    // 复制任务数据（避免指针共享）
    memcpy(task_data, &current_task, sizeof(drone_task_t));
    return true;
}

bool has_valid_mission_data(void) {
    return has_mission_data && (current_task.waypoint_count > 0);
}


// 更新后的任务处理函数
static void handle_task_command(uint8_t *content, uint16_t content_len) {
    uint16_t task_code;
    uint16_t json_len;
    
    // 解析任务指令头
    memcpy(&task_code, content, sizeof(task_code));
    memcpy(&json_len, content + 2, sizeof(json_len));

    // 验证JSON长度
    if (json_len > (content_len - 4) || json_len == 0) {
        ESP_LOGE(TAG, "无效JSON长度: %d", json_len);
        send_task_response(task_code, 1, 0x0001, current_task.target_id); // 参数错误
        return;
    }

    // 提取并解析JSON
    char *json_text = malloc(json_len + 1);
    if (!json_text) {
        ESP_LOGE(TAG, "内存分配失败");
        send_task_response(task_code, 1, 0x0002, current_task.target_id); // 内存不足
        return;
    }
    
    memcpy(json_text, content + 4, json_len);
    json_text[json_len] = '\0';

    ESP_LOGI(TAG, "收到任务指令: 代码=0x%04X", task_code);
    ESP_LOGD(TAG, "任务JSON: %s", json_text);

    bool parse_success = parse_task_json(json_text);
    free(json_text);

    if (!parse_success) {
        ESP_LOGE(TAG, "JSON解析失败");
        send_task_response(task_code, 1, 0x0003, current_task.target_id); // 数据解析错误
        return;
    }

    // 根据任务类型执行不同操作 等待任务释放
    switch (task_code) {
        case 0x0001: // 目标装订
            ESP_LOGI(TAG, "目标装订成功");
            ESP_LOGI(TAG, "目标ID: %d, 类型: %d", 
                    current_task.target_id, current_task.target_type);
            ESP_LOGI(TAG, "位置: (%.7f,%.7f), 高度: %.2f",
                    current_task.target_lon, current_task.target_lat, current_task.target_alt);
            ESP_LOGI(TAG, "航点数: %d", current_task.waypoint_count);
            
            // 更新全局状态
            
            // has_target = true;
            // mavlink_set_has_target(has_target);
            has_mission_data = true;

            // 确定需要等待哪些确认
            need_guide_ack = true;  // 总是需要航点GPS确认
            need_mission_ack = (current_task.waypoint_count > 0);  // 有航点时需要航线确认

            // 开始跟踪响应状态
            start_task_response_tracking(task_code, current_task.target_id, 
                                       need_guide_ack, need_mission_ack);

            // 1. 发送目标GPS给飞控
            ESP_LOGI(TAG, "发送目标GPS到飞控...");
            mavlink_send_guide_target(current_task.target_lat, current_task.target_lon, current_task.target_alt, current_task.target_type);
            
            // 2. 启动航线上传流程
            if (current_task.waypoint_count > 0) {
                ESP_LOGI(TAG, "启动航线上传流程...");
                mavlink_set_mission_upload_state(current_task.waypoint_count, 0, MISSION_STATE_UPLOADING);
                mavlink_send_mission_count(target_system, target_component, current_task.waypoint_count);
            } else {
                ESP_LOGW(TAG, "没有航线数据,仅发送目标GPS");
            }
            ESP_LOGI(TAG, "等待飞控请求航点数据...");

            // 需要收到飞控发来的数据来判断是否成功
            // ESP_LOGI(TAG, "等待飞控确认...");
            // 注意：这里不再立即发送 send_task_response，而是等待确认
            // send_task_response(task_code, 0, 0, current_task.target_id); // 临时发送成功
            break;

        case 0x0002: // 取消装订=清除航线+清除目标点装订
            // 确定需要等待哪些确认
            need_guide_ack = true;  // 总是需要航点GPS确认
            need_mission_ack = true;  // 有航点时需要航线确认

            // 开始跟踪响应状态
            start_task_response_tracking(task_code, current_task.target_id, 
                                       need_guide_ack, need_mission_ack);

            ESP_LOGI(TAG, "开始清除航线");
            // 1. 发送目标GPS给飞控
            ESP_LOGI(TAG, "发送不正常目标GPS到飞控,以取消目标点装订...");
            // 设置非法经纬高"alt": 10, "lon": 1810000000, "lat": 910000000
            static int32_t invalid_target_lat = 910000000;
            static int32_t invalid_target_lon = 1810000000;
            static int32_t invalid_target_alt = 10;
            static int8_t target_type = 0;
            mavlink_send_guide_target(invalid_target_lat, invalid_target_lon, invalid_target_alt, target_type);
            mavlink_mission_clear_all();
            mavlink_set_mission_upload_state(0, 0, MISSION_STATE_CLEARING);
            
            // send_task_response(task_code, 0, 0, current_task.target_id); // 成功
            // 更新全局状态
            // has_target = false;
            // mavlink_set_has_target(has_target);
            has_mission_data = false;
            
            break;

        case 0x0011: // 执行攻击
            if (!mavlink_get_has_target()) {
                ESP_LOGE(TAG, "未装订目标，无法执行");
                send_task_response(task_code, 1, 0x0004, current_task.target_id); // 无效状态
                return;
            }
            
            // 检查是否已有攻击任务在执行
            if (attack_exec_tracker.is_executing) {
                ESP_LOGW(TAG, "攻击任务正在执行中，拒绝新任务");
                send_task_response(task_code, 1, 0x0030, current_task.target_id); // 设备忙
                return;
            }

            ESP_LOGI(TAG, "开始执行攻击任务...");

            // 初始化攻击执行跟踪器
            memset(&attack_exec_tracker, 0, sizeof(attack_exec_tracker));
            attack_exec_tracker.is_executing = true;
            attack_exec_tracker.task_code = task_code;
            attack_exec_tracker.target_id = current_task.target_id;
            attack_exec_tracker.timeout_limit = 10000; // 4秒总超时, 0.5s 之后起飞

            execute_attack_task();
            // send_task_response(task_code, 0, 0, current_task.target_id); // 成功
            break;

        default:
            ESP_LOGW(TAG, "未知任务代码: 0x%04X", task_code);
            send_task_response(task_code, 1, 0x0005, current_task.target_id); // 不支持的操作
            break;
    }
}


// 打印 sockaddr 结构体
void print_sockaddr(const struct sockaddr *addr) {
    if (addr == NULL) {
        printf("Address is NULL\n");
        return;
    }

    char ip_str[INET6_ADDRSTRLEN];
    uint16_t port = 0;

    if (addr->sa_family == AF_INET) { // IPv4
        struct sockaddr_in *addr_in = (struct sockaddr_in *)addr;
        inet_ntop(AF_INET, &(addr_in->sin_addr), ip_str, INET_ADDRSTRLEN);
        port = ntohs(addr_in->sin_port);
        printf("IPv4 Address: %s, Port: %u\n", ip_str, port);
    } else if (addr->sa_family == AF_INET6) { // IPv6
        struct sockaddr_in6 *addr_in6 = (struct sockaddr_in6 *)addr;
        inet_ntop(AF_INET6, &(addr_in6->sin6_addr), ip_str, INET6_ADDRSTRLEN);
        port = ntohs(addr_in6->sin6_port);
        printf("IPv6 Address: %s, Port: %u\n", ip_str, port);
    } else {
        printf("Unknown address family: %d\n", addr->sa_family);
    }
}

// 发送任务响应
static void send_task_response(uint16_t task_code, uint8_t result, uint16_t error_code, uint16_t target_id) {
    const size_t msg_len = 32; // 固定长度
    uint8_t tx_buffer[32] = {0};
    
    // 消息头
    tx_buffer[0] = MSG_HEADER1;  // 0xEB
    tx_buffer[1] = MSG_HEADER2;  // 0x90
    
    // 发送方设备ID (字节2-5)
    uint32_t device_id = g_device_id;
    memcpy(tx_buffer + 2, &device_id, sizeof(device_id));
    
    // 循环序号 (字节6)
    tx_buffer[6] = seq_counter++;
    
    // 消息类型标志 (字节7-8)
    uint16_t msg_flag = MSG_TASK_RESPONSE; // 应该定义为0x4011
    memcpy(&tx_buffer[7], &msg_flag, sizeof(msg_flag));
    
    // 内容长度 (字节9-10)
    uint16_t content_len = 19; // 固定内容长度 (字节11-29)
    memcpy(&tx_buffer[9], &content_len, sizeof(content_len));
    
    // 任务情况代码 (字节11-12)
    memcpy(&tx_buffer[11], &task_code, sizeof(task_code));
    
    // 执行情况 (字节13)
    tx_buffer[13] = result;
    
    // 错误码 (字节14-15)
    memcpy(&tx_buffer[14], &error_code, sizeof(error_code));
    
    // 执行任务序号 (字节16-17) - 新增字段
    memcpy(&tx_buffer[16], &target_id, sizeof(target_id));
    
    // 预留字段 (字节18-29) - 已经通过memset(tx_buffer, 0, 32)初始化为0
    
    // 消息校验 (字节30-31) - 计算字节0-29的校验值
    uint16_t checksum = calculate_checksum(tx_buffer, 30);
    memcpy(&tx_buffer[30], &checksum, sizeof(checksum));
    
    // 发送响应
    if (udp_sock >= 0) {
        int sent = sendto(udp_sock, tx_buffer, msg_len, 0,
                    (struct sockaddr *)&remote_addr, sizeof(remote_addr));
        
        // 打印 remote_addr
        printf("Remote address info: ");
        print_sockaddr((struct sockaddr *)&remote_addr);

        if (sent < 0) {
            ESP_LOGE(TAG, "响应发送失败: errno=%d", errno);
        } else {

            ESP_LOGI(TAG, "已发送任务响应至：: 代码=0x%04X, 结果=%d, 错误码=0x%04X, 目标ID=0x%04X, 字节数=%d",
                    task_code, result, error_code, target_id, sent);
        }
    }
}

// 发送入群请求消息
static void send_join_request(void) {
    msg_header_t msg;
    unsigned char signature[MBEDTLS_MPI_MAX_SIZE];
    size_t sig_len = 0;
    
    // 构造消息头
    msg.header1 = MSG_HEADER1;
    msg.header2 = MSG_HEADER2;
    msg.sender_id = g_device_id;  
    msg.seq_num = seq_counter++;
    msg.msg_flag = MSG_DRONE_JOIN_REQ; 
    
    // 生成数字签名
    if (!sign_message(signature, &sig_len)) {
        ESP_LOGE(TAG, "签名生成失败，发送未签名消息");
        sig_len = 0; // 如果签名失败，发送无签名消息
    }
    
    msg.content_len = sig_len;  
    ESP_LOGI(TAG, "内容长度: %d", msg.content_len);

    if (udp_sock >= 0) {
        // 计算总消息长度：消息头 + 内容 + 签名 + 校验和
        size_t header_content_len = sizeof(msg_header_t);
        size_t total_len = header_content_len + sig_len + 2; // +2 for checksum
        uint8_t *full_msg = malloc(total_len);
        
        if (full_msg) {
            // 复制消息头和内容
            memcpy(full_msg, &msg, sizeof(msg_header_t));
            
            // 添加签名（如果有）
            if (sig_len > 0) {
                memcpy(full_msg + header_content_len, signature, sig_len);
                ESP_LOGI(TAG, "已添加数字签名，长度: %d", sig_len);
            }
            
            // 计算校验和（包括消息头、内容和签名）
            uint16_t checksum = calculate_checksum(full_msg, total_len - 2);
            memcpy(full_msg + total_len - 2, &checksum, 2);
            
            // 发送完整消息
            int err = sendto(udp_sock, full_msg, total_len, 0,
                           (struct sockaddr *)&remote_addr, sizeof(remote_addr));
            
            if (err < 0) {
                ESP_LOGE(TAG, "Error sending join request: errno %d", errno);
            } else {
                char addr_str[INET6_ADDRSTRLEN];
                inet_ntoa_r(remote_addr.sin_addr, addr_str, sizeof(addr_str));
                ESP_LOGI(TAG, "Join request sent to %s:%d (total: %d bytes, signature: %d bytes)", 
                        addr_str, ntohs(remote_addr.sin_port), total_len, sig_len);
                
                // 打印发送的消息内容用于调试
                print_hex_string("发送消息", full_msg, total_len > 64 ? 64 : total_len);
                // ESP_LOGI(TAG, "校验和: %04X", checksum);
            }
            
            free(full_msg);
        } else {
            ESP_LOGE(TAG, "内存分配失败");
        }
    }
}

// 发送状态信息
static void send_status_info(void) {
    /* ===================== 频率追踪 - 新增 ===================== */
    static uint32_t call_count = 0;
    static uint32_t last_call_time = 0;
    static uint8_t last_sent_seq = 255; // 上次发送序列号追踪
    uint32_t current_time = esp_log_timestamp();
    uint32_t time_diff = current_time - last_call_time;
    
    call_count++;
    // ESP_LOGI(TAG, "=== 函数调用 #%" PRIu32 " === 时间:%" PRIu32 "ms 间隔:%" PRIu32 "ms", 
    //          call_count, current_time, time_diff);
    last_call_time = current_time;
    
    
    if (!can_send_status()) {  // 使用新的判断函数
        ESP_LOGD(TAG, "未连接到机群，跳过状态发送 (state=%d)", fleet_state);
        return;
    }

    // 定期更新mount_point
    update_mount_point_from_nfc();
    
    // 协议规定的固定长度
    const size_t total_msg_len = 64; // 11(头) + 51(内容) + 2(校验)
    uint8_t tx_buffer[64] = {0};
    
    /* ===================== 调试信息 - 准备阶段 ===================== */
    bool mavlink_self_check = mavlink_get_self_check_status(); // 飞控自检状态
    bool mount_point_ready = (mount_point != 0); // 挂点状态：不为0表示已挂载
    bool status_is_self_check_passed = mavlink_self_check && mount_point_ready; // 综合判断

    bool status_is_unlocked = mavlink_get_armed_status();
    bool status_has_target = mavlink_get_has_target();
    current_target_lon = current_task.target_lon;
    current_target_lat = current_task.target_lat;
    current_target_alt = current_task.target_alt;
    current_target_id = current_task.target_id;
    current_target_type = current_task.target_type;

    ESP_LOGI(TAG, "=== 状态检查详情 ===");
    ESP_LOGI(TAG, "mavlink_self_check: %s (%d)", mavlink_self_check ? "PASS" : "FAIL", mavlink_self_check);
    ESP_LOGI(TAG, "mount_point: %d", mount_point);
    ESP_LOGI(TAG, "mount_point_ready: %s (%d)", mount_point_ready ? "READY" : "NOT_READY", mount_point_ready);
    ESP_LOGI(TAG, "status_is_self_check_passed: %s (%d)", status_is_self_check_passed ? "PASS" : "FAIL", status_is_self_check_passed);
    ESP_LOGI(TAG, "===================");


    ESP_LOGI(TAG, "准备发送状态信息...");
    ESP_LOGI(TAG, "当前状态: 自检=%d 解锁=%d 目标锁定=%d 弹药类型=%d", 
             status_is_self_check_passed, status_is_unlocked, status_has_target, current_ammo_type);
    ESP_LOGI(TAG, "目标信息: ID=%d 类型=%d 位置=(%.7f,%.7f) 高度=%.2fm",
             current_task.target_id, current_task.target_type,
             current_task.target_lon, current_task.target_lat, current_task.target_alt);

    /* ===================== 消息头构造 ===================== */
    tx_buffer[0] = MSG_HEADER1;
    tx_buffer[1] = MSG_HEADER2;
    
    // ESP_LOGI(TAG, "准备发送设备ID: %lu (0x%08lX)", g_device_id, g_device_id);

    uint32_t device_id = g_device_id;
    memcpy(&tx_buffer[2], &device_id, sizeof(device_id));
    
    // ESP_LOGI(TAG, "发送的字节: [2]=%02X [3]=%02X [4]=%02X [5]=%02X", 
    //          tx_buffer[2], tx_buffer[3], tx_buffer[4], tx_buffer[5]);

    static uint8_t seq_num = 0;
    tx_buffer[6] = seq_num;

    /* ===================== 序列号追踪 - 新增 ===================== */
    // ESP_LOGI(TAG, "序列号: 当前=%d 上次发送=%d", seq_num, last_sent_seq);
    
    uint16_t msg_flag = MSG_DRONE_STATUS;
    memcpy(&tx_buffer[7], &msg_flag, sizeof(msg_flag));
    
    uint16_t content_len = 51;
    memcpy(&tx_buffer[9], &content_len, sizeof(content_len));

    /* ===================== 状态内容构造 ===================== */
    // 设备状态 (2字节)
    uint16_t device_status = 0;
    device_status |= (status_is_self_check_passed << 0);  // 自检通过
    device_status |= (status_is_unlocked << 1);
    device_status |= (status_has_target << 2);
    memcpy(&tx_buffer[11], &device_status, sizeof(device_status));
    
    // 挂点信息
    // uint16_t mount_point = 0;
    memcpy(&tx_buffer[13], &mount_point, sizeof(mount_point));
    tx_buffer[15] = current_ammo_type;
    
    // 目标信息
    memcpy(&tx_buffer[16], &current_target_id, sizeof(current_target_id));
    tx_buffer[18] = current_target_type;
    
    uint32_t longitude = (uint32_t)(current_target_lon * 1e7);
    memcpy(&tx_buffer[19], &longitude, sizeof(longitude));
    
    uint32_t latitude = (uint32_t)(current_target_lat * 1e7);
    memcpy(&tx_buffer[23], &latitude, sizeof(latitude));
    
    int16_t altitude = (int16_t)(current_target_alt * 100);
    memcpy(&tx_buffer[27], &altitude, sizeof(altitude));
    
    memset(&tx_buffer[29], 0, 33); // 预留字段清零

    /* ===================== 校验和计算 ===================== */
    uint16_t checksum = 0;
    for (int i = 0; i < 62; i++) {
        checksum += tx_buffer[i];
    }
    memcpy(&tx_buffer[62], &checksum, sizeof(checksum));

    /* ===================== 调试信息 - 发送前 ===================== */
    // ESP_LOGI(TAG, "完整消息结构:");
    // print_hex_string("消息头(0-10)", &tx_buffer[0], 11);
    // print_hex_string("设备状态(11-12)", &tx_buffer[11], 2);
    // print_hex_string("目标位置(19-26)", &tx_buffer[19], 8);
    // print_hex_string("校验和(62-63)", &tx_buffer[62], 2);
    
    // ESP_LOGI(TAG, "校验和计算: 0x%04X", checksum);
    // ESP_LOGI(TAG, "即将发送 %d 字节数据到 %s:%d", 
    //          total_msg_len, 
    //          inet_ntoa(remote_addr.sin_addr), 
    //          ntohs(remote_addr.sin_port));

    /* ===================== 发送前最后检查 - 新增 ===================== */
    uint32_t pre_send_time = esp_log_timestamp();
    // ESP_LOGI(TAG, "即将发送: 序号=%d 校验和=0x%04X 准备耗时=%" PRIu32 "ms", 
    //          seq_num, checksum, pre_send_time - current_time);


    /* ===================== 消息发送 ===================== */
    if (udp_sock >= 0) {
        int sent = sendto(udp_sock, tx_buffer, total_msg_len, 0,
                         (struct sockaddr *)&remote_addr, sizeof(remote_addr));
        
        uint32_t post_send_time = esp_log_timestamp();

        if (sent < 0) {
            ESP_LOGE(TAG, "发送失败! errno=%d (%s) 发送耗时=%" PRIu32 "ms", 
                     errno, strerror(errno), post_send_time - pre_send_time);
            // ESP_LOGI(TAG, "=== 函数退出 #%" PRIu32 " === 原因: 发送失败", call_count);
        } else {
            // ESP_LOGI(TAG, "成功发送 %d/%d 字节 (序号:%d) 发送耗时=%" PRIu32 "ms", 
            //         sent, total_msg_len, seq_num, post_send_time - pre_send_time);
            
            /* ===================== 重复发送检查 - 新增 ===================== */
            if (seq_num == last_sent_seq) {
                ESP_LOGW(TAG, "!!! 警告: 重复发送相同序列号 %d !!!", seq_num);
            }
            last_sent_seq = seq_num;
            
            seq_num++; // 仅当发送成功时递增序列号
            
            /* ===================== 总耗时统计 - 新增 ===================== */
            // uint32_t total_time = post_send_time - current_time;
            // ESP_LOGI(TAG, "=== 函数完成 #%" PRIu32 " === 总耗时=%" PRIu32 "ms 新序号=%d", 
            //          call_count, total_time, seq_num);
        }
    } else {
        ESP_LOGE(TAG, "UDP套接字无效!");

        // ESP_LOGI(TAG, "=== 函数退出 #%" PRIu32 " === 原因: 套接字无效", call_count);
    }
}

static void update_mount_point_from_nfc(void) {
    static uint32_t last_update_time = 0;
    uint32_t current_time = esp_log_timestamp();
    
    // 每5秒检查一次NFC数据更新（避免过于频繁）
    if (current_time - last_update_time < 5000) {
        return;
    }
    last_update_time = current_time;
    
    if (nfc_enabled) {
        uint32_t nfc_mount_point = get_nfc_mount_point();
        if (is_nfc_data_valid()) {
            if (mount_point != (uint16_t)nfc_mount_point) {
                ESP_LOGI(TAG, "NFC挂点更新: %d -> %lu", mount_point, nfc_mount_point);
                mount_point = (uint16_t)nfc_mount_point;
                last_nfc_update = current_time;
            }
        } else {
            // NFC数据无效时的处理策略
            if (mount_point != 0) {  // 只有当前不为0时才需要重置和打印日志
                ESP_LOGW(TAG, "长时间无有效NFC数据，挂点从 %d 恢复为默认值 0", mount_point);
                mount_point = 0;  // 恢复为默认值0
            } else {
                ESP_LOGD(TAG, "长时间无有效NFC数据，挂点保持默认值 0");
            }
        }
    }
}

static void handle_received_message(uint8_t *rx_buffer, int len) {
    // 基本消息头检查
    if (len < sizeof(msg_header_t)) {
        ESP_LOGW(TAG, "消息长度不足，小于消息头");
        return;
    }

    // 调试信息输出
    // ESP_LOGI(TAG, "原始消息前32字节:");
    // ESP_LOGI(TAG, "字节数%d:", len);
    // print_hex_string("RAW", rx_buffer, len > 32 ? 32 : len);

    // 解析消息头
    msg_header_t *header = (msg_header_t *)rx_buffer;
    
    // 检查消息头标志
    if (header->header1 != MSG_HEADER1 || header->header2 != MSG_HEADER2) {
        ESP_LOGW(TAG, "消息头不正确: 0x%02X 0x%02X", header->header1, header->header2);
        return;
    }

    // 提取关键字段（小端序直接读取）
    uint32_t sender_id = header->sender_id;
    uint16_t msg_flag = header->msg_flag;
    uint16_t content_len = header->content_len;
    uint8_t seq_num = header->seq_num;

    // ESP_LOGI(TAG, "解析消息: ID=0x%08"PRIx32", 类型=0x%04X, 序列号=%d, 内容长度=%d",
    //         sender_id, msg_flag, seq_num, content_len);

    // 根据消息类型进行不同处理
    switch (msg_flag) {
        case MSG_FLEET_BROADCAST:       // 0x1001 机群广播消息
            handle_broadcast_message(rx_buffer, len, header);
            break;
            
        case MSG_DRONE_JOIN_ACK:        // 0x1012 入群确认消息
            handle_join_ack_message(rx_buffer, len, header);
            break;
            
        case MSG_TASK_COMMAND:          // 0x4001 任务指令消息
            handle_task_command_message(rx_buffer, len, header);
            break;
            
        default:
            ESP_LOGW(TAG, "未知消息类型: 0x%04X", msg_flag);
            break;
    }
}

// 处理广播消息（带签名）
static void handle_broadcast_message(uint8_t *rx_buffer, int len, msg_header_t *header) {
    uint32_t current_tick = xTaskGetTickCount();
    // 使用线程安全的方式读取上次广播时间
    uint32_t last_tick = get_last_broadcast_tick_safe();

    ESP_LOGI(TAG, "current_tick:(%lu ticks)", current_tick);
    
    // 计算与上次广播的间隔
    if (last_tick != 0) {
        uint32_t interval_ticks = current_tick - last_tick;
        uint32_t interval_ms = interval_ticks * portTICK_PERIOD_MS;
        ESP_LOGI(TAG, "广播间隔: %lu ms (%lu ticks)", interval_ms, interval_ticks);
        
        if (interval_ms > 3000) {
            ESP_LOGW(TAG, "⚠️ 广播间隔较长: %lu ms", interval_ms);
        }
    } else {
        ESP_LOGI(TAG, "首次收到广播消息");
    }
    
    // 线程安全地更新广播时间戳
    update_last_broadcast_tick_safe(current_tick);
    
    // 计算最小长度要求：消息头 + 签名 + 校验和
    size_t min_len = sizeof(msg_header_t) + RSA_SIGNATURE_LEN + 2;
    if (len < min_len) {
        ESP_LOGW(TAG, "广播消息长度不足，期望至少%d字节", min_len);
        return;
    }

    // 校验和验证（整个消息，除最后2字节）
    uint16_t received_checksum;
    memcpy(&received_checksum, rx_buffer + len - 2, 2);
    uint16_t calculated_checksum = calculate_checksum(rx_buffer, len - 2);
    
    if (received_checksum != calculated_checksum) {
        ESP_LOGW(TAG, "广播消息校验和验证失败");
        return;
    }

    // 签名验证（消息头 + 签名）
    uint8_t *signature = rx_buffer + sizeof(msg_header_t);
    if (!verify_message_signature(rx_buffer, sizeof(msg_header_t), signature, RSA_SIGNATURE_LEN)) {
        ESP_LOGW(TAG, "广播消息签名验证失败");
        return;
    }

    // 状态机处理
    
    if (fleet_state == FLEET_STATE_DISCONNECTED) {
        set_fleet_state(FLEET_STATE_JOINING);
        send_join_request();
        ESP_LOGI(TAG, "申请入群");
    } else if (fleet_state == FLEET_STATE_JOINING) {
        // 正在入群过程中，继续等待JOIN_ACK
        ESP_LOGI(TAG, "正在入群中，等待确认...");
    } else {
        // 已连接状态，这是正常的心跳广播
        ESP_LOGI(TAG, "心跳广播正常");
    }
}

// 处理入群确认消息（带签名）
static void handle_join_ack_message(uint8_t *rx_buffer, int len, msg_header_t *header) {
    // 计算最小长度要求：消息头 + 签名 + 校验和
    size_t min_len = sizeof(msg_header_t) + RSA_SIGNATURE_LEN + 2;
    if (len < min_len) {
        ESP_LOGW(TAG, "入群确认消息长度不足，期望至少%d字节", min_len);
        return;
    }

    // 校验和验证
    uint16_t received_checksum;
    memcpy(&received_checksum, rx_buffer + len - 2, 2);
    uint16_t calculated_checksum = calculate_checksum(rx_buffer, len - 2);
    
    if (received_checksum != calculated_checksum) {
        ESP_LOGW(TAG, "入群确认消息校验和验证失败");
        return;
    }

    // 签名验证
    uint8_t *signature = rx_buffer + sizeof(msg_header_t);
    if (!verify_message_signature(rx_buffer, sizeof(msg_header_t), signature, RSA_SIGNATURE_LEN)) {
        ESP_LOGW(TAG, "入群确认消息签名验证失败");
        return;
    }

    // ESP_LOGI(TAG, "收到有效的入群确认消息");
    // 只有在JOINING状态才处理JOIN_ACK
    if (fleet_state == FLEET_STATE_JOINING) {
        ESP_LOGI(TAG, "收到有效的入群确认消息，成功加入机群");
        set_fleet_state(FLEET_STATE_CONNECTED);

        // 重新连接，重新设置发送状态，使得机群管理设备认为此飞机可以分配任务(目前重新连接不清空飞控状态，但是可以覆盖)
        reset_current_task();

        // 停止重连机制
        rejoin_tracker.is_rejoining = false;
        rejoin_tracker.rejoin_attempt_count = 0;
        ESP_LOGI(TAG, "重连机制已停止");
    } else {
        ESP_LOGW(TAG, "收到JOIN_ACK但当前不在入群状态: %d", fleet_state);
    }
}

// 处理任务指令消息（无签名）
static void handle_task_command_message(uint8_t *rx_buffer, int len, msg_header_t *header) {
    // 计算最小长度要求：消息头 + 内容 + 校验和
    size_t min_len = sizeof(msg_header_t) + header->content_len + 2;
    if (len < min_len) {
        ESP_LOGW(TAG, "任务指令消息长度不足，期望至少%d字节", min_len);
        return;
    }

    // 校验和验证（仅验证消息头+内容）
    uint16_t received_checksum;
    memcpy(&received_checksum, rx_buffer + len - 2, 2);
    uint16_t calculated_checksum = calculate_checksum(rx_buffer, len - 2);
    
    if (received_checksum != calculated_checksum) {
        ESP_LOGW(TAG, "任务指令消息校验和验证失败");
        return;
    }

    // 任务指令消息不需要签名验证
    ESP_LOGI(TAG, "收到有效的任务指令消息");
    
    // 提取任务内容并处理
    uint8_t *content = rx_buffer + sizeof(msg_header_t);
    handle_task_command(content, header->content_len);
}

// 攻击任务执行实现
void execute_attack_task(void) {
    ESP_LOGI(TAG, "====== 开始执行攻击任务 ======");

    
    // 初始化攻击任务管理器
    attack_manager.state = ATTACK_STATE_START;
    attack_manager.start_time = xTaskGetTickCount();
    attack_manager.last_action_time = attack_manager.start_time;
    attack_manager.is_active = true;
    
    ESP_LOGI(TAG, "攻击任务已启动，进入解锁阶段");
}


// 攻击任务状态机处理
static void process_attack_state_machine(void) {
    if (!attack_manager.is_active || !attack_exec_tracker.is_executing) {
        return;
    }
    
    uint32_t current_tick = xTaskGetTickCount();
    uint32_t elapsed_ms = (current_tick - attack_manager.last_action_time) * portTICK_PERIOD_MS;
    uint32_t total_elapsed_ms = (current_tick - attack_manager.start_time) * portTICK_PERIOD_MS;
    
    // 总超时检查
    if (total_elapsed_ms > attack_exec_tracker.timeout_limit) {
        ESP_LOGE(TAG, "攻击任务总超时: %lu ms", total_elapsed_ms);
        finish_attack_task(false, 0x0020); // 超时错误
        return;
    }

    bool is_guide = !current_task.strike_type; //0为地面发射，1为空中发射默认为地面发射

    // 添加ESP32打印信息，显示当前发射模式
    if (is_guide == 0) {
        ESP_LOGI(TAG, "=== 发射模式信息 ===");
        ESP_LOGI(TAG, "当前模式: 空中发射模式");
        ESP_LOGI(TAG, "Strike Type: 0 (Ground Launch)");
        ESP_LOGI(TAG, "==================");
    } else if (is_guide == 1) {
        ESP_LOGI(TAG, "=== 发射模式信息 ===");
        ESP_LOGI(TAG, "当前模式: 地基发射模式");  
        ESP_LOGI(TAG, "Strike Type: 1 (Air Launch)");
        ESP_LOGI(TAG, "==================");
    } else {
        ESP_LOGW(TAG, "=== 发射模式信息 ===");
        ESP_LOGW(TAG, "警告: 未知发射模式");
        ESP_LOGW(TAG, "Strike Type: %d", current_task.strike_type);
        ESP_LOGW(TAG, "==================");
    }

    // 可选：添加更详细的调试信息
    ESP_LOGD(TAG, "is_guide 变量值: %d", is_guide);

    switch (attack_manager.state) {
        case ATTACK_STATE_START:
            if (is_guide) {
                attack_manager.state = ATTACK_STATE_GUIDE;
            } else {
                attack_manager.state = ATTACK_STATE_THROW;
            }
            break;

        case ATTACK_STATE_THROW:
            switch_to_throw_mode();
            attack_manager.last_action_time = current_tick;
            if (mavlink_state.custom_mode == 18) {
                attack_manager.state = ATTACK_STATE_ARMING;
            }
            break;

        case ATTACK_STATE_GUIDE:
            switch_to_guide_mode();
            attack_manager.last_action_time = current_tick;
            if (mavlink_state.custom_mode == 4) {
                attack_manager.state = ATTACK_STATE_ARMING;
            }
            break;

        case ATTACK_STATE_ARMING:
            // 发送解锁指令
            mavlink_force_arm(true);
            attack_manager.last_action_time = current_tick;
            if (is_guide) {
                attack_manager.state = ATTACK_STATE_TAKEOFF;
                ESP_LOGI(TAG, "已发送解锁指令，等待500ms后起飞");
            } else {
                if (mavlink_get_armed_status()) {
                    finish_attack_task(true, 0x0000);
                }
            }
            break;
            
        case ATTACK_STATE_TAKEOFF:
            // 检查解锁状态
            if (mavlink_get_armed_status()) {
                if (!attack_exec_tracker.armed_confirmed) {
                    attack_exec_tracker.armed_confirmed = true;
                    ESP_LOGI(TAG, "解锁成功确认，500ms后发送起飞指令");
                    attack_manager.last_action_time = current_tick;
                    disarm_success(true, 0x0012);
                }
                
                // 解锁成功后等待500ms再起飞
                if (attack_exec_tracker.armed_confirmed && elapsed_ms >= 500) {
                    send_takeoff_command(10.0);  // 起飞到10米高度
                    attack_manager.last_action_time = current_tick;
                    attack_manager.state = ATTACK_STATE_MISSION_MODE;
                    ESP_LOGI(TAG, "已发送起飞指令，等待2000ms后切换航线模式");
                }
            } else {
                // 检查解锁超时（比如5秒）
                if (elapsed_ms > 3000) {
                    ESP_LOGE(TAG, "解锁超时失败");
                    finish_attack_task(false, 0x0012); // 解锁失败
                    return;
                }
            }
            break;
            
        case ATTACK_STATE_MISSION_MODE:
            if (elapsed_ms >= 2000) {  // 等待2000ms
                switch_to_mission_mode();
                attack_manager.last_action_time = current_tick;
                attack_manager.state = ATTACK_STATE_EXECUTING;
                ESP_LOGI(TAG, "已切换到航线模式，开始执行任务");
            }
            break;
            
        case ATTACK_STATE_EXECUTING:
            // 任务执行中，等待飞控完成
            // 可以通过监控MAVLink消息来判断任务完成
            break;
            
        case ATTACK_STATE_COMPLETED:
            attack_manager.is_active = false;
            ESP_LOGI(TAG, "攻击任务完成");
            break;
            
        default:
            break;
    }
}

static void finish_attack_task(bool success, uint16_t error_code) {
    if (!attack_exec_tracker.is_executing) {
        return;
    }
    
    // 发送最终任务响应
    uint8_t result = success ? 0 : 1;
    send_task_response(attack_exec_tracker.task_code, result, error_code, attack_exec_tracker.target_id);
    
    // 清理状态
    attack_manager.state = ATTACK_STATE_COMPLETED;
    attack_manager.is_active = false;
    
    memset(&attack_exec_tracker, 0, sizeof(attack_exec_tracker));
    
    uint32_t total_time = (xTaskGetTickCount() - attack_manager.start_time) * portTICK_PERIOD_MS;
    ESP_LOGI(TAG, "攻击任务%s，总耗时: %lu ms", 
             success ? "成功完成" : "失败", total_time);
}

static void disarm_success(bool success, uint16_t error_code) {
    if (!attack_exec_tracker.is_executing) {
        return;
    }
    
    // 发送最终任务响应
    uint8_t result = success ? 0 : 1;
    send_task_response(attack_exec_tracker.task_code, result, error_code, attack_exec_tracker.target_id);
    
    uint32_t total_time = (xTaskGetTickCount() - attack_manager.start_time) * portTICK_PERIOD_MS;
    ESP_LOGI(TAG, "攻击任务%s，总耗时: %lu ms", 
             success ? "成功完成" : "失败", total_time);
}

// UDP 接收任务
static void udp_server_task(void *pvParameters) {
    char rx_buffer[2048]; // 增大缓冲区以容纳签名
    char addr_str[128];
    int addr_family = AF_INET;
    int ip_protocol = IPPROTO_IP;

    while (!wifi_connected) {
        ESP_LOGI(TAG, "UDP server waiting for WiFi connection...");
        vTaskDelay(pdMS_TO_TICKS(1000));
    }

    
    dest_addr.sin_addr.s_addr = htonl(INADDR_ANY);;
    dest_addr.sin_family = AF_INET;
    dest_addr.sin_port = htons(MCAST_PORT);
    
    udp_sock = socket(addr_family, SOCK_DGRAM, ip_protocol);
    if (udp_sock < 0) {
        ESP_LOGE(TAG, "Unable to create socket: errno %d", errno);
        vTaskDelete(NULL);
        return;
    }
    

    // 绑定到组播端口
    if (bind(udp_sock, (struct sockaddr *)&dest_addr, sizeof(dest_addr)) < 0) {
        ESP_LOGE(TAG, "Socket unable to bind: errno %d", errno);
        close(udp_sock);
        vTaskDelete(NULL);
        return;
    }

    // 加入组播组

    mreq.imr_multiaddr.s_addr = inet_addr(MCAST_GRP);
    mreq.imr_interface.s_addr = htonl(INADDR_ANY);
    // mreq.imr_ifindex = if_nametoindex("ens33");
    if (setsockopt(udp_sock, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) < 0) {
        ESP_LOGE(TAG, "Failed to join multicast group");
        close(udp_sock);
        vTaskDelete(NULL);
        return;
    }
    
    
    ESP_LOGI(TAG, "Multicast UDP server started, listening on %s:%d", MCAST_GRP, MCAST_PORT);
    static int cnt = 0;
    static int cnt2 = 0;
    while (1) {
        if (!wifi_connected) {
            ESP_LOGW(TAG, "WiFi disconnected, UDP server exiting...");
            break;
        }

        struct sockaddr_storage source_addr;
        socklen_t socklen = sizeof(source_addr);
        int len = recvfrom(udp_sock, rx_buffer, sizeof(rx_buffer) - 1, 0, 
                          (struct sockaddr *)&source_addr, &socklen);                 
        if (len < 0) {
            ESP_LOGE(TAG, "recvfrom failed: errno %d", errno);
            // 网络错误时重置状态
            set_fleet_state(FLEET_STATE_DISCONNECTED);
            break;
        } else {
            // 获取发送方IP地址
            if (source_addr.ss_family == PF_INET) {
                memcpy(&remote_addr, &source_addr, sizeof(remote_addr));
                inet_ntoa_r(((struct sockaddr_in *)&source_addr)->sin_addr, 
                           addr_str, sizeof(addr_str) - 1);
            }
            
            // ESP_LOGI(TAG, "第%d次: 从 %s:%d 收到 %d 字节数据",cnt2,  addr_str, ntohs(remote_addr.sin_port), len);
            cnt2++;
            // 处理消息（包含签名验证）
            handle_received_message((uint8_t*)rx_buffer, len);
        }
        cnt++;
    }
    
    if (udp_sock != -1) {
        ESP_LOGE(TAG, "Shutting down socket and restarting...");
        shutdown(udp_sock, 0);
        close(udp_sock);
        udp_sock = -1;
    }
    vTaskDelete(NULL);
}


// 定期状态发送任务
static void status_sender_task(void *pvParameters) {
    while (1) {
        vTaskDelay(pdMS_TO_TICKS(100));  // 改为100ms检查一次，更及时
        
        // 处理攻击任务状态机
        process_attack_state_machine();
        
        // 检查任务响应超时
        check_task_timeout();
        
        // 每500ms发送一次状态
        static uint32_t status_counter = 0;
        if (++status_counter >= 5) {
            status_counter = 0;
            if (is_connected_to_fleet()) {
                send_status_info();
            }
        }
    }
}

// 2. 修改event_handler函数
static void event_handler(void* arg, esp_event_base_t event_base,
                                int32_t event_id, void* event_data)
{
    if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START) {
        esp_wifi_connect();
    } else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED) {
        wifi_connected = false;
        set_fleet_state(FLEET_STATE_DISCONNECTED); // 重置连接状态
        
        // 不再限制重试次数，持续尝试重连
        ESP_LOGI(TAG, "WiFi disconnected, will retry connection...");
        
        // 使用延时避免过于频繁的重连
        vTaskDelay(pdMS_TO_TICKS(2000)); // 等待2秒
        esp_wifi_connect();
        
        // 设置失败标志，但不停止重连
        if (s_retry_num >= EXAMPLE_ESP_MAXIMUM_RETRY) {
            xEventGroupSetBits(s_wifi_event_group, WIFI_FAIL_BIT);
            s_retry_num = 0; // 重置计数器，继续尝试
        } else {
            s_retry_num++;
        }
        
    } else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) {
        ip_event_got_ip_t* event = (ip_event_got_ip_t*) event_data;
        ESP_LOGI(TAG, "got ip:" IPSTR, IP2STR(&event->ip_info.ip));
        local_ip = event->ip_info.ip.addr;
        s_retry_num = 0;
        wifi_connected = true;
        xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT);
        xEventGroupClearBits(s_wifi_event_group, WIFI_FAIL_BIT);
    }
}

// 3. 添加WiFi重连任务
static void wifi_reconnect_task(void *pvParameters) {
    uint32_t reconnect_interval = 5000; // 初始5秒重连间隔
    uint32_t max_interval = 30000; // 最大30秒重连间隔
    
    while (1) {
        vTaskDelay(pdMS_TO_TICKS(reconnect_interval));
        
        if (!wifi_connected) {
            ESP_LOGI(TAG, "WiFi reconnect task: Attempting to reconnect...");
            
            // 获取当前WiFi状态
            wifi_ap_record_t ap_info;
            esp_err_t ret = esp_wifi_sta_get_ap_info(&ap_info);
            
            if (ret != ESP_OK) {
                // 未连接到AP，尝试重新连接
                ESP_LOGI(TAG, "Not connected to AP, initiating connection...");
                esp_wifi_connect();
                
                // 逐渐增加重连间隔，避免过于频繁
                if (reconnect_interval < max_interval) {
                    reconnect_interval += 5000;
                }
            }
        } else {
            // 已连接，重置重连间隔
            reconnect_interval = 5000;
        }
    }
}

static void fleet_watchdog_task(void *pvParameters) {
    static int chaoshiCnt = 0;
    static uint32_t last_disconnect_tick = 0;
    while (1) {
        uint32_t now = xTaskGetTickCount();
        // ESP_LOGI(TAG, "watchdog: (%lu ticks), now: %lu, last_broadcast_tick: %lu, BROADCAST_TIMEOUT_TICKS: (%lu ticks)", now - last_broadcast_tick, now, last_broadcast_tick, BROADCAST_TIMEOUT_TICKS);
        // 原子性读取关键变量
        uint32_t last_tick = get_last_broadcast_tick_safe();
        fleet_connection_state_t current_state = fleet_state;

        // 计算时间差，处理可能的溢出
        uint32_t tick_diff;
        if (now >= last_tick) {
            tick_diff = now - last_tick;
        } else {
            uint32_t reverse_diff = last_tick - now;
            
            if (reverse_diff < pdMS_TO_TICKS(1000)) {
                // 如果时间差小于1秒，很可能是时序竞态，忽略本次检查
                ESP_LOGD(TAG, "检测到时序竞态(now=%lu < last=%lu, diff=%lu)，忽略本次检查", 
                        now, last_tick, reverse_diff);
                vTaskDelay(pdMS_TO_TICKS(1000));
                continue;  // 跳过本轮检查
            } else {
                // 真正的tick溢出情况
                tick_diff = (UINT32_MAX - last_tick) + now + 1;
                ESP_LOGI(TAG, "处理tick计数器溢出: last=%lu, now=%lu, diff=%lu", 
                        last_tick, now, tick_diff);
            }
        }
        
        // 使用局部变量进行日志输出，确保一致性
        ESP_LOGI(TAG, "watchdog: diff=%lu, now=%lu, last=%lu, timeout=%lu, state=%d", 
                tick_diff, now, last_tick, BROADCAST_TIMEOUT_TICKS, current_state);
        // 计算运行时间（秒）
        uint32_t uptime_seconds = now / configTICK_RATE_HZ;
        
        // 计算距离上次掉线的时间（秒）
        uint32_t since_last_disconnect_seconds = 0;
        if (last_disconnect_tick > 0) {
            uint32_t since_disconnect_ticks = (now >= last_disconnect_tick) ? 
                                             (now - last_disconnect_tick) : 
                                             ((UINT32_MAX - last_disconnect_tick) + now + 1);
            since_last_disconnect_seconds = since_disconnect_ticks / configTICK_RATE_HZ;
        } else {
            since_last_disconnect_seconds = uptime_seconds; // 从未掉线过
        }
        
        // 简化的日志输出
        // ESP_LOGI(TAG, "运行: %lu秒 | 掉线次数: %d | 距上次掉线: %lu秒 | 状态: %d", 
        //         uptime_seconds, chaoshiCnt, since_last_disconnect_seconds, current_state);

        switch (current_state) {
            case FLEET_STATE_CONNECTED:
                // 检查广播超时
                if (tick_diff > BROADCAST_TIMEOUT_TICKS) {
                    ESP_LOGW(TAG, "确认广播超时: %lu > %lu，开始重连流程", 
                            tick_diff, BROADCAST_TIMEOUT_TICKS);
                    chaoshiCnt++;
                    last_disconnect_tick = now;
                    set_fleet_state(FLEET_STATE_DISCONNECTED);
                    
                //     // 初始化重连状态
                //     rejoin_tracker.rejoin_attempt_count = 0;
                //     rejoin_tracker.is_rejoining = true;
                //     rejoin_tracker.last_rejoin_attempt = now;
                }
                break;
                
            case FLEET_STATE_DISCONNECTED:
                // 自动重连逻辑
                if (rejoin_tracker.is_rejoining) {
                    uint32_t time_since_last_attempt = now - rejoin_tracker.last_rejoin_attempt;
                    
                    if (time_since_last_attempt >= pdMS_TO_TICKS(REJOIN_RETRY_INTERVAL_MS)) {
                        if (rejoin_tracker.rejoin_attempt_count < MAX_REJOIN_ATTEMPTS) {
                            rejoin_tracker.rejoin_attempt_count++;
                            rejoin_tracker.last_rejoin_attempt = now;
                            
                            ESP_LOGI(TAG, "自动重连尝试 %d/%d", 
                                    rejoin_tracker.rejoin_attempt_count, MAX_REJOIN_ATTEMPTS);
                            
                            set_fleet_state(FLEET_STATE_JOINING);
                            send_join_request();
                        } else {
                            ESP_LOGE(TAG, "重连达到最大尝试次数，停止自动重连");
                            rejoin_tracker.is_rejoining = false;
                        }
                    }
                } else {
                    ESP_LOGD(TAG, "断开状态，等待广播消息或手动重连...");
                }
                break;
                
            case FLEET_STATE_JOINING:
                // 入群超时检查
                uint32_t join_timeout = pdMS_TO_TICKS(3000); // 2秒入群超时
                // if (rejoin_tracker.is_rejoining && 
                //     (now - rejoin_tracker.last_rejoin_attempt > join_timeout)) {
                //     ESP_LOGW(TAG, "入群请求超时，回到断开状态");
                //     set_fleet_state(FLEET_STATE_DISCONNECTED);
                // }
                if (tick_diff > BROADCAST_TIMEOUT_TICKS) {
                    ESP_LOGW(TAG, "正在连接但是，广播超时: %lu > %lu", 
                            tick_diff, BROADCAST_TIMEOUT_TICKS);
                    chaoshiCnt++;
                    last_disconnect_tick = now;
                    set_fleet_state(FLEET_STATE_DISCONNECTED);
                    
                }
                break;
        }
        vTaskDelay(pdMS_TO_TICKS(1000));

    }
}


// ===== 回调函数实现 =====

// GUIDE_ACK回调函数实现
static void on_guide_ack_received(bool success) {
    ESP_LOGI(TAG, "收到航点GPS装订确认: %s", success ? "成功" : "失败");
    handle_guide_ack_response(success);
}

// MISSION_ACK回调函数实现  
static void on_mission_ack_received(bool success) {
    ESP_LOGI(TAG, "收到航线上传确认: %s", success ? "成功" : "失败");
    handle_mission_ack_response(success);
}


void wifi_init_sta(void)
{
    s_wifi_event_group = xEventGroupCreate();

    ESP_ERROR_CHECK(esp_netif_init());

    ESP_ERROR_CHECK(esp_event_loop_create_default());
    esp_netif_create_default_wifi_sta();

    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    ESP_ERROR_CHECK(esp_wifi_init(&cfg));

    esp_event_handler_instance_t instance_any_id;
    esp_event_handler_instance_t instance_got_ip;
    ESP_ERROR_CHECK(esp_event_handler_instance_register(WIFI_EVENT,
                                                        ESP_EVENT_ANY_ID,
                                                        &event_handler,
                                                        NULL,
                                                        &instance_any_id));
    ESP_ERROR_CHECK(esp_event_handler_instance_register(IP_EVENT,
                                                        IP_EVENT_STA_GOT_IP,
                                                        &event_handler,
                                                        NULL,
                                                        &instance_got_ip));

    wifi_config_t wifi_config = {
        .sta = {
            .ssid = EXAMPLE_ESP_WIFI_SSID,
            .password = EXAMPLE_ESP_WIFI_PASS,
            /* Authmode threshold resets to WPA2 as default if password matches WPA2 standards (password len => 8).
             * If you want to connect the device to deprecated WEP/WPA networks, Please set the threshold value
             * to WIFI_AUTH_WEP/WIFI_AUTH_WPA_PSK and set the password with length and format matching to
             * WIFI_AUTH_WEP/WIFI_AUTH_WPA_PSK standards.
             */
            .threshold.authmode = ESP_WIFI_SCAN_AUTH_MODE_THRESHOLD,
            .sae_pwe_h2e = ESP_WIFI_SAE_MODE,
            .sae_h2e_identifier = EXAMPLE_H2E_IDENTIFIER,
        },
    };

    
    ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA) );
    ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config) );
    ESP_ERROR_CHECK(esp_wifi_start() );
    ESP_ERROR_CHECK(esp_wifi_set_ps(WIFI_PS_NONE));

    ESP_LOGI(TAG, "wifi_init_sta finished.");

    /* Waiting until either the connection is established (WIFI_CONNECTED_BIT) or connection failed for the maximum
     * number of re-tries (WIFI_FAIL_BIT). The bits are set by event_handler() (see above) */
    // EventBits_t bits = xEventGroupWaitBits(s_wifi_event_group,
    //         WIFI_CONNECTED_BIT | WIFI_FAIL_BIT,
    //         pdFALSE,
    //         pdFALSE,
    //         portMAX_DELAY);

    EventBits_t bits = xEventGroupWaitBits(s_wifi_event_group,
            WIFI_CONNECTED_BIT | WIFI_FAIL_BIT,
            pdFALSE,
            pdFALSE,
            pdMS_TO_TICKS(10000)); // 只等待10秒，不是永久等待

    xTaskCreate(wifi_reconnect_task, "wifi_reconnect", 4096, NULL, 5, &wifi_reconnect_task_handle);

    ESP_LOGI(TAG, "WiFi reconnect task started");

    // 初始化签名验证（即使WiFi未连接也初始化）
    if (!init_signature_verification()) {
        ESP_LOGW(TAG, "数字签名验证初始化失败，将跳过签名验证");
    }

    if (!init_signature_signing()) {
        ESP_LOGW(TAG, "数字签名功能初始化失败，将发送未签名消息");
    }

    // 注册MAVLink回调函数
    mavlink_set_guide_ack_callback(on_guide_ack_received);
    mavlink_set_mission_ack_callback(on_mission_ack_received);
    ESP_LOGI(TAG, "MAVLink回调函数已注册");

    // 启动核心任务（即使WiFi未连接也启动）
    xTaskCreate(mavlink_task, "mavlink_task", 8192, NULL, 7, NULL);
    ESP_LOGI(TAG, "MAVLink task started");

    // 创建一个任务来管理UDP相关任务的启动
    xTaskCreate(network_services_monitor_task, "net_monitor", 4096, NULL, 5, NULL);
    ESP_LOGI(TAG, "Network services monitor task started");

    if (bits & WIFI_CONNECTED_BIT) {
        ESP_LOGI(TAG, "WiFi connected to SSID:%s", EXAMPLE_ESP_WIFI_SSID);
    } else {
        ESP_LOGI(TAG, "WiFi not connected yet, will keep trying in background...");
    }
}


static TaskHandle_t udp_server_task_handle = NULL;
static TaskHandle_t status_sender_task_handle = NULL;
static TaskHandle_t fleet_watchdog_task_handle = NULL;

static void network_services_monitor_task(void *pvParameters) {
    bool services_running = false;
    
    while (1) {
        vTaskDelay(pdMS_TO_TICKS(1000)); // 每秒检查一次
        
        if (wifi_connected && !services_running) {
            ESP_LOGI(TAG, "WiFi connected, starting network services...");
            
            // 启动UDP服务器任务
            if (udp_server_task_handle == NULL) {
                xTaskCreate(udp_server_task, "udp_server", 12288, NULL, 7, &udp_server_task_handle);
                ESP_LOGI(TAG, "UDP server task started");
            }
            
            // 启动状态发送任务
            if (status_sender_task_handle == NULL) {
                xTaskCreate(status_sender_task, "status_sender", 4096, NULL, 7, &status_sender_task_handle);
                ESP_LOGI(TAG, "Status sender task started");
            }
            
            // 启动机群看门狗任务
            if (fleet_watchdog_task_handle == NULL) {
                xTaskCreate(fleet_watchdog_task, "fleet_watchdog", 4096, NULL, 4, &fleet_watchdog_task_handle);
                ESP_LOGI(TAG, "Fleet watchdog task started");
            }
            
            services_running = true;
            
        } else if (!wifi_connected && services_running) {
            ESP_LOGI(TAG, "WiFi disconnected, stopping network services...");
            
            // 停止UDP相关任务
            if (udp_server_task_handle != NULL) {
                vTaskDelete(udp_server_task_handle);
                udp_server_task_handle = NULL;
                ESP_LOGI(TAG, "UDP server task stopped");
            }
            
            if (status_sender_task_handle != NULL) {
                vTaskDelete(status_sender_task_handle);
                status_sender_task_handle = NULL;
                ESP_LOGI(TAG, "Status sender task stopped");
            }
            
            if (fleet_watchdog_task_handle != NULL) {
                vTaskDelete(fleet_watchdog_task_handle);
                fleet_watchdog_task_handle = NULL;
                ESP_LOGI(TAG, "Fleet watchdog task stopped");
            }
            
            // 关闭UDP socket
            if (udp_sock != -1) {
                close(udp_sock);
                udp_sock = -1;
            }
            
            services_running = false;
        }
    }
}

static esp_err_t nfc_system_init(void) {
    ESP_LOGI(TAG, "初始化NFC系统...");
    
    // 初始化RC522 I2C
    esp_err_t ret = rc522_i2c_init();
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "NFC I2C初始化失败: %s", esp_err_to_name(ret));
        return ret;
    }
    
    // 读取版本寄存器验证通信
    uint8_t version;
    ret = rc522_read_register(RC522_REG_VERSION, &version, 1);
    if (ret == ESP_OK) {
        ESP_LOGI(TAG, "RC522版本: 0x%02X", version);
        
        if (version == 0x91 || version == 0x92 || version == 0x88) {
            ESP_LOGI(TAG, "RC522检测成功！");
        } else {
            ESP_LOGW(TAG, "意外的版本号，但通信正常");
        }
    } else {
        ESP_LOGE(TAG, "读取RC522版本失败: %s", esp_err_to_name(ret));
        return ret;
    }
    
    // 初始化RFID模式
    ret = rc522_init_rfid_mode();
    if (ret != ESP_OK) {
        ESP_LOGE(TAG, "RFID模式初始化失败: %s", esp_err_to_name(ret));
        return ret;
    }
    
    ESP_LOGI(TAG, "NFC系统初始化完成");
    nfc_enabled = true;
    
    return ESP_OK;
}

void app_main(void)
{
    //Initialize NVS
    esp_err_t ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
      ESP_ERROR_CHECK(nvs_flash_erase());
      ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK(ret);

    // Initialize uart
    mavlink_handler_init();

    // 创建任务跟踪器互斥锁
    task_tracker_mutex = xSemaphoreCreateMutex();
    if (task_tracker_mutex == NULL) {
        ESP_LOGE(TAG, "创建任务跟踪器互斥锁失败!");
    }

    // 初始化MAVLink状态保护
    mavlink_state_init();

    ret = nfc_system_init();
    if (ret == ESP_OK) {
        ESP_LOGI(TAG, "启动NFC数据读取任务...");
        xTaskCreate(nfc_read_task, "nfc_read_task", 4096, NULL, 5, NULL);
    } else {
        ESP_LOGW(TAG, "NFC系统初始化失败，将使用默认挂点值");
        nfc_enabled = false;
    }

    ESP_LOGI(TAG, "ESP_WIFI_MODE_STA - Attack Drone ID: 0x%08" PRIx32, g_device_id);
    wifi_init_sta();
}

// 辅助函数实现
static void print_hex_string(const char* label, const unsigned char* data, size_t len)
{
    char hex_str[len * 2 + 1];
    for (size_t i = 0; i < len; i++) {
        sprintf(&hex_str[i * 2], "%02x", data[i]);
    }
    hex_str[len * 2] = '\0';
    ESP_LOGI(TAG, "%s: %s", label, hex_str);
}

static void print_mbedtls_error(const char* function, int error_code)
{
    char error_buf[100];
    mbedtls_strerror(error_code, error_buf, sizeof(error_buf));
    ESP_LOGE(TAG, "%s 失败: -0x%04x (%s)", function, -error_code, error_buf);
}